#include <cmath>
#include "geometry.hpp"

/* @brief Calculate the angle between two vectors.
   @param a The first vector.
   @param b The second vector.

   This routine takes two vectors, and returns the smaller of the two
   angles between them.  The calculation may degenerate if one of the
   vectors has a length close to zero.
*/
double util::angle(util::point a, util::point b) {
  a.normalise();
  b.normalise();
  return std::acos(a * b);
}

/*
  @brief Find the area of a triangle.
  @param a A vertex one the triangle.
  @param b A vertex one the triangle.
  @param c A vertex one the triangle.

  This routine finds the area of a triangle on the xy-plane.  The
  triangle does not need to be a right-triangle or axis-aligned.

  From http://www.btinternet.com/~se16/hgb/triangle.htm
*/
double util::planar_triangle_area(const util::point& a, const util::point& b, const util::point& c) {
  double ab = b.x() * a.y() - a.x() * b.y();
  double bc = c.x() * b.y() - b.x() * c.y();
  double ac = a.x() * c.y() - c.x() * a.y();

  double area = std::abs(ab + bc + ac) * 0.5;
  return area;
}

/* @brief Find the intersection of two lines.
   @param a1 A point on the first line.
   @param a2 Another point on the first line.
   @param b1 A point on the second line.
   @param b2 Another point on the second line.
   @param ia Set to true if the intersection is in the line segment a
   @param ib Set to true if the intersection is in the line segment b

   This routine finds the intersection of two lines in the xy-plane.
   It does not account for the case of parrallel or coincident
   lines.

   From htp://astronomy.swin.edu.au/~pbourke/geometry/lineline2d/
*/
util::point util::planar_line_intersection(const util::point& a1, const util::point& a2, const util::point& b1, const util::point& b2, bool& ia, bool& ib) {
  double x1 = a1.x();
  double x2 = a2.x();
  double x3 = b1.x();
  double x4 = b2.x();
  double y1 = a1.y();
  double y2 = a2.y();
  double y3 = b1.y();
  double y4 = b2.y();

  double d = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
  double ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3)) / d;
  double ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3)) / d;

  double x = x1 + ua * (x2 - x1);
  double y = y1 + ua * (y2 - y1);

  ia = ua > 0.0 && ua < 1.0;
  ib = ub > 0.0 && ub < 1.0;

  return util::point(x, y);
}

/* @brief Rotation of a vector in the xy-plane.
   @param v The vector from the origin to rotate.
   @param angle The angle to rotate by.

   This routine takes a vector from the origin in the xy-plane and
   rotates it by the supplied angle in the counter-clockwise
   direction.

   Sine, cosine and arctangent from the standard math library are
   used, so the type used in the template should be convertable to a
   plain floating-point type.

   This method came from the web, but I lost the url.
*/
util::point util::planar_rotation(const util::point& v, double angle) {
  double alpha = std::atan2(v.y(), v.x()) + angle;
  double length = v.length();
  return util::point(length * std::cos(alpha), length * std::sin(alpha));
}
